#include <arch.h>
#include <asm_macros.h>
#include <plat.h>

/* The offset to add to get ascii for numerals '0 - 9' */
#define ASCII_OFFSET_NUM	0x30

.global init_crash_buf
.global do_unhandled_exception
.global do_panic
.global asm_print_str
.global asm_print_hex

#ifdef REPORT_CRASH

#define REG_SIZE	0x8
#define CB_COUNT	10
#define CB_SIZE		(CB_COUNT * REG_SIZE)
#define CB_OFS_X0	0
#define CB_OFS_X1	REG_SIZE
#define CB_OFS_X2	(REG_SIZE*2)
#define CB_OFS_X3	(REG_SIZE*3)
#define CB_OFS_X4	(REG_SIZE*4)
#define CB_OFS_X5	(REG_SIZE*5)
#define CB_OFS_X6	(REG_SIZE*6)
#define CB_OFS_X7	(REG_SIZE*7)
#define CB_OFS_sp	(REG_SIZE*8)
#define CB_OFS_lr	(REG_SIZE*9)

/*
 * The crash_buf is in the data section and not in .bss
 * even though it is zero-init. In particular, this allows
 * the exception functions to start using this buffer before
 * the runtime memory is initialized for images which do not
 * need to copy the .data section from ROM to RAM.
 */
.section .data.crash_buf ; .align 5
crash_buf:
	.space ((PLAT_CORE_COUNT) * (CB_SIZE)), 0

.macro prepare_crash_buf_save_x0_to_x7_sp_lr
	mov	sp, x0
	mrs	x0, tpidr_el3
	str	x1, [x0, #REG_SIZE]
	mov	x1, sp
	str	x1, [x0]
	stp	x2, x3, [x0, #REG_SIZE*2]
	stp	x4, x5, [x0, #REG_SIZE*4]
	stp	x6, x7, [x0, #REG_SIZE*6]
	mrs	x1, spsel
	msr	spsel, #0	/*sp of worktime*/
	mov	x2, sp
	msr	spsel, x1
	stp	x2, x30, [x0, #REG_SIZE*8]
.endm

.macro dump_reg id, skip=0
.if \id < 8
	adr	x4, msg_x\id
	bl	asm_print_str
 .if \skip == 0
	mrs	x7, tpidr_el3
 .endif
	ldr	x4, [x7, #REG_SIZE*\id]
	bl	asm_print_hex
.else
 .if \id > 7 && \id < 30
	adr	x4, msg_x\id
	bl	asm_print_str
	mov	x4, x\id
	bl	asm_print_hex
 .else
  .if \id == 30
	adr	x4, msg_x30
	bl	asm_print_str
   .if \skip == 0
	mrs	x7, tpidr_el3
   .endif
	ldr	x4, [x7, #CB_OFS_lr]
	bl	asm_print_hex
  .endif
 .endif
.endif
.endm

/*
 * macro helper to dump register: sp, x0 - x30
 */
.macro show_registers
	mrs	x7, tpidr_el3

	adr	x4, sp_msg
	bl	asm_print_str
	ldr	x4, [x7, #CB_OFS_sp]
	bl	asm_print_hex
	bl	asm_print_newl

	dump_reg 0, 1
	dump_reg 1, 1
	dump_reg 2, 1
	dump_reg 3, 1
	bl	asm_print_newl
	dump_reg 4, 1
	dump_reg 5, 1
	dump_reg 6, 1
	dump_reg 7, 1
	bl	asm_print_newl
	dump_reg 8, 1
	dump_reg 9, 1
	dump_reg 10, 1
	dump_reg 11, 1
	bl	asm_print_newl
	dump_reg 12, 1
	dump_reg 13, 1
	dump_reg 14, 1
	dump_reg 15, 1
	bl	asm_print_newl
	dump_reg 16, 1
	dump_reg 17, 1
	dump_reg 18, 1
	dump_reg 19, 1
	bl	asm_print_newl
	dump_reg 20, 1
	dump_reg 21, 1
	dump_reg 22, 1
	dump_reg 23, 1
	bl	asm_print_newl
	dump_reg 24, 1
	dump_reg 25, 1
	dump_reg 26, 1
	dump_reg 27, 1
	bl	asm_print_newl
	dump_reg 28, 1
	dump_reg 29, 1
	dump_reg 30, 1
	bl	asm_print_newl
.endm

.macro dump_reg_mem id, skip=0
.if \id < 8
 .if \skip == 0
	mrs	x7, tpidr_el3
 .endif
	ldr	x0, [x7, #REG_SIZE*\id]
	adr	x1, msg_x\id
	bl	crash_dump_memory
.else
 .if \id > 7 && \id < 30
	mov	x0, x\id
	adr	x1, msg_x\id
	bl	crash_dump_memory
 .else
  .if \id == 30
   .if \skip == 0
	mrs	x7, tpidr_el3
   .endif
	ldr	x0, [x7, #CB_OFS_lr]
	adr	x1, msg_x30
	bl	crash_dump_memory
  .endif
 .endif
.endif
.endm

.macro dump_stack
	mrs	x7, tpidr_el3
	ldr	x0, [x7, #CB_OFS_sp]
	adr	x1, sp_msg
	bl	crash_dump_memory
.endm

/*
 * macro helper to dump accessible memory
 * pointed by any register of {x0 - x30, sp}
 */
.macro show_memory
	adr	x4, show_mem_str
	bl	asm_print_str
	dump_reg_mem 0
	dump_reg_mem 1
	dump_reg_mem 2
	dump_reg_mem 3
	dump_reg_mem 4
	dump_reg_mem 5
	dump_reg_mem 6
	dump_reg_mem 7
	dump_reg_mem 8
	dump_reg_mem 9
	dump_reg_mem 10
	dump_reg_mem 11
	dump_reg_mem 12
	dump_reg_mem 13
	dump_reg_mem 14
	dump_reg_mem 15
	dump_reg_mem 16
	dump_reg_mem 17
	dump_reg_mem 18
	dump_reg_mem 19
	dump_reg_mem 20
	dump_reg_mem 21
	dump_reg_mem 22
	dump_reg_mem 23
	dump_reg_mem 24
	dump_reg_mem 25
	dump_reg_mem 26
	dump_reg_mem 27
	dump_reg_mem 28
	dump_reg_mem 29
	dump_reg_mem 30
	dump_stack
.endm

.section .rodata.exception_str, "aS"
pc_msg:  .asciz " pc : "
pstate_msg: .asciz "pstate : "
sp_msg:  .asciz " sp : "
esr_msg: .asciz " esr: "
elr_msg: .asciz " elr: "
far_msg: .asciz " far: "

.section .rodata.crash_str, "aS"
msg_x0:  .asciz " x0 : "
msg_x1:  .asciz " x1 : "
msg_x2:  .asciz " x2 : "
msg_x3:  .asciz " x3 : "
msg_x4:  .asciz " x4 : "
msg_x5:  .asciz " x5 : "
msg_x6:  .asciz " x6 : "
msg_x7:  .asciz " x7 : "
msg_x8:  .asciz " x8 : "
msg_x9:  .asciz " x9 : "
msg_x10: .asciz " x10: "
msg_x11: .asciz " x11: "
msg_x12: .asciz " x12: "
msg_x13: .asciz " x13: "
msg_x14: .asciz " x14: "
msg_x15: .asciz " x15: "
msg_x16: .asciz " x16: "
msg_x17: .asciz " x17: "
msg_x18: .asciz " x18: "
msg_x19: .asciz " x19: "
msg_x20: .asciz " x20: "
msg_x21: .asciz " x21: "
msg_x22: .asciz " x22: "
msg_x23: .asciz " x23: "
msg_x24: .asciz " x24: "
msg_x25: .asciz " x25: "
msg_x26: .asciz " x26: "
msg_x27: .asciz " x27: "
msg_x28: .asciz " x28: "
msg_x29: .asciz " x29: "
msg_x30: .asciz " x30: "

/*
 * int is_addr_accessible(u64 addr);
 * In:  x0  - memory address to check
 * Out: return 1 on YES else 0
 * Clobber: x0, x1
 */
func is_addr_accessible
	mov_imm	x1, PLAT_MEM_BASE
	cmp	x1, x0
	b.hi	1f
	mov_imm	x1, (PLAT_MEM_BASE + PLAT_MEM_SIZE)
	cmp	x0, x1
	b.hi	1f
	mov	x0, #1
	ret
1:	/*invalid addr*/
	mov	x0, #0
	ret
endfunc is_addr_accessible

#define MEM_DUMP_DEPTH	0x80
/*
 * void crash_dump_memory(u32 addr, char *str);
 * Clobber: x0 - x7, x30, sp
 */
func crash_dump_memory
	mov	x6, x0
	mov	x7, x1
	mov	sp, x30
	bl	is_addr_accessible
	cbz	x0, 10f
	bl	asm_print_newl
	mov	x4, x7
	/*print 'str: addr'*/
	bl	asm_print_str
	mov	x4, x6
	bl	asm_print_hex32
	/*fix 'bottom' and 'top'*/
	mov	x0, x6
	and	x0, x6, #~31
	sub	x0, x0, #MEM_DUMP_DEPTH
	mov_imm	x1, PLAT_MEM_BASE
	cmp	x1, x0
	b.ls	1f
	mov	x0, x1
1:
	add	x3, x6, #31
	and	x3, x3, #~31
	add	x3, x3, #MEM_DUMP_DEPTH
	mov_imm	x1, (PLAT_MEM_BASE + PLAT_MEM_SIZE)
	cmp	x3, x1
	b.ls	2f
	mov	x3, x1
2:
	mov	x6, x0	/*bottom*/
	mov	x7, x3	/*top*/
3:
	cmp	x7, x6
	b.ls	5f
	ands	x0, x6, #31
	b.ne	4f
	bl	asm_print_newl
	mov	x4, x6
	bl	asm_print_hex32
	adr	x4, colon_str
	bl	asm_print_str
4:
	ldr	w4, [x6], #4
	bl	asm_print_hex32
	adr	x4, space_str
	bl	asm_print_str
	b	3b
5:
	bl	asm_print_newl
10:
	mov	x30, sp
	ret
endfunc crash_dump_memory

.section .rodata.dumpmem_str, "aS"
show_mem_str:
	.asciz "\nMemory Dumps\n"
colon_str:  .asciz ": "
space_str:  .asciz " "

/*
 * void do_report_crash(void);
 * Clobber: x0 - x7, x30, sp
 * no return
 */
func do_report_crash
	show_registers
	show_memory

	b	crash_panic
endfunc do_report_crash

/*
 * void init_crash_buf(void)
 * Clobber: x0, x1, x2, x30
 */
func init_crash_buf
	mov	x3, x30
	mrs	x0, mpidr_el1
	bl	plat_get_core_pos
	ldr	x2, =crash_buf
	mov_imm	x1, CB_SIZE
	madd	x0, x0, x1, x2
	/* Store crash buffer address in tpidr_el3 */
	msr	tpidr_el3, x0
	ret	x3
endfunc init_crash_buf

/*
 * void do_unhandled_exception(void);
 * stack less, noreturn
 * Clobber: x0,x1,x2,x3,x4,x5,x6,x7,x30,sp
 */
func do_unhandled_exception
	prepare_crash_buf_save_x0_to_x7_sp_lr

    bl  plat_set_crash_stack

	adr	x4, plat_err_str
	bl	asm_print_str

	bl	asm_print_core
	adr	x4, fault_addr_str
	bl	asm_print_str
	mrs	x4, far_el3
	bl	asm_print_hex
	bl	asm_print_newl

	adr	x4, pstate_msg
	bl	asm_print_str
	mrs	x4, spsr_el3
	bl	asm_print_hex
	bl	asm_print_newl

	adr	x4, esr_msg
	bl	asm_print_str
	mrs	x4, esr_el3
	bl	asm_print_hex
	bl	asm_print_newl

	adr	x4, elr_msg
	bl	asm_print_str
	mrs	x4, elr_el3
	bl	asm_print_hex
	bl	asm_print_newl

	b	do_report_crash
endfunc do_unhandled_exception

#else /*REPORT_CRASH else*/

/*
 * void do_unhandled_exception(void);
 * stack less, noreturn
 * Clobber: x0,x1,x2,x3,x4,x5,x30
 */
func do_unhandled_exception
	b   .
endfunc do_unhandled_exception

#endif /*REPORT_CRASH*/

.section .rodata.rev_err_str, "aS"
plat_err_str:
	.asciz "\nException Reporting\n"

core_str:
	.asciz "Core #"
fault_addr_str:
	.asciz " Fault at addr : 0x"
newl_str:
	.asciz "\n"

.section .rodata.panic_str, "aS"
panic_msg: .asciz " PANIC at PC : 0x"

/*
 * void crash_panic(void);
 * noreturn
 */
func crash_panic
_panic_loop:
	b	_panic_loop
endfunc crash_panic

/*
 * void asm_print_core(void);
 * Clobber; x0, x4, x6, x30
 */
func asm_print_core
	mov	x6, x30
	adr	x4, core_str
	bl	asm_print_str
	mrs	x0, mpidr_el1
	bl	plat_get_core_pos
	add	x0, x0, #ASCII_OFFSET_NUM
	bl	console_putc
	ret	x6
endfunc asm_print_core

/*
 * print newline on console
 * Clobber: x4
 */
func asm_print_newl
	adr	x4, newl_str
	b	asm_print_str
endfunc asm_print_newl

/*
 * This function prints a string from address in x4.
 * In: x4 = pointer to string.
 * Clobber: x30, x0, x1, x2, x3
 */
func asm_print_str
	mov	x3, x30
1:
	ldrb	w0, [x4], #0x1
	cbz	x0, 2f
	bl	console_putc
	b	1b
2:
	ret	x3
endfunc asm_print_str

/*
 * This function prints a hexadecimal number in x4.
 * In: x4 = the hexadecimal to print.
 *     x5 = number of bits to convert to ascii
 * Clobber: x30, x0, x5, x1, x2, x3
 */
func asm_print_data
	mov	x3, x30
	ands	x5, x5, #~3
	b.eq	3f
1:
	sub	x5, x5, #4
	lsrv	x0, x4, x5
	and	x0, x0, #0xf
	cmp	x0, #0xA
	b.lo	2f
	/* Add by 0x27 in addition to ASCII_OFFSET_NUM
	 * to get ascii for characters 'a - f'.
	 */
	add	x0, x0, #0x27
2:
	add	x0, x0, #ASCII_OFFSET_NUM
	bl	console_putc
	cbnz	x5, 1b
3:
	ret	x3
endfunc asm_print_data

/*
 * This function prints a hexadecimal number in x4.
 * In: x4 = the hexadecimal to print.
 * Clobber: x30, x0, x5, x1, x2, x3
 */
func asm_print_hex
	mov	x5, #64  /* No of bits to convert to ascii */
	b	asm_print_data
endfunc asm_print_hex

/*
 * This function prints a hexadecimal number in x4.
 * In: x4 = the hexadecimal to print.
 * Clobber: x30, x0, x5, x1, x2, x3
 */
func asm_print_hex32
	mov	x5, #32  /* No of bits to convert to ascii */
	b	asm_print_data
endfunc asm_print_hex32

